package crtl;


/**
 *
 * @author benjamin
 */
public class ProportionalLoveLightCtrl extends ProportionalCtrl {

	private final int _finishedThreshold = 150; 
	
	private final double[][] _controllerMatrix = {
		{ 0,  0,  0,  -1,  1,  0,  0,  2},
		{ 2,  0,  0,   1, -1,  0,  0,  0}
	};
	
	private final double[] _constants = {0, 0};
	private boolean _finished = false;
	
	public ProportionalLoveLightCtrl() {
		super();
		
		System.out.println("Proportional - Love Light Controller initialized");
	}
	
	@Override
	protected boolean finished() {
		return _finished;
	}

	@Override
	protected double[] getConstants() {
		return _constants;
	}

	@Override
	protected double[][] getControllMatrix() {
		return _controllerMatrix;
	}

	@Override
	protected double[] fetchAndPrepareSensorValues() {
		double[] lightValues = readLightSensors();
		lightValues = calcAverage(lightValues);
		lightValues = normalize(lightValues, _finishedThreshold, findMax(lightValues));
		checkFinished(lightValues);
		return lightValues;
	}

	private void checkFinished(double[] lightValues) {
		if ((lightValues[Direction.S_FRONT_LEFT.getIndex()] == 0) && (lightValues[Direction.S_FRONT_RIGHT.getIndex()] == 0)) {
			_finished = true;
		}
	}
}
